#ifndef BTL_VIEWGEOMETRY_FIVEPOINTPOSE
#define BTL_VIEWGEOMETRY_FIVEPOINTPOSE

#include <Eigen/Core>
#include <Eigen/Geometry>
#include <vector>

namespace btl
{
namespace viewgeometry
{

/// Implements the 5-point-pose algorithm as described in
///   Recent Developments on Direct Relative Orientation
///     Henrik Stewénius, Christopher Engels and David Nistér
///   2006, Elsevier
class FivePointStewenius
{
   public:
      enum ComputeOptions
      {
         ApplyRankFix     = 1 << 0,
         NormaliseResults = 1 << 1,
      };

      FivePointStewenius();

      template <typename MT1, typename MT2>
      FivePointStewenius(const Eigen::MatrixBase<MT1>& x1, const Eigen::MatrixBase<MT2>& x2, int opts = 0);

      template <typename MT1, typename MT2>
      void compute(const Eigen::MatrixBase<MT1>& x1, const Eigen::MatrixBase<MT2>& x2, int opts = 0);

      int numSolutions() const;
      const Eigen::Matrix3d& solution(int i) const;
   private:
      void computeInternal(int opts);
      Eigen::MatrixXd buildMonomialMatrix(const Eigen::MatrixXd& Es);

      Eigen::MatrixXd _epipolarConstraintMatrix;
      Eigen::Matrix3d _solutions[10];
      int _numSolutions;
};

} // namespace viewgeometry
} // namespace btl

namespace btl
{
   using viewgeometry::FivePointStewenius;
}

// ====================================================================
// === Implementation

namespace btl
{
namespace viewgeometry
{

/// Constructs a coplanarity constraint matrix
/// which encodes the constraints:
///   x2.transpose() * E * x1 = 0
template <int Dim, typename MT1, typename MT2>
struct CoplanarConstraintHelper {
   static void f(
         Eigen::MatrixXd& cs,
         const Eigen::MatrixBase<MT1>& x1,
         const Eigen::MatrixBase<MT2>& x2);
};
template <typename MT1, typename MT2>
struct CoplanarConstraintHelper<2,MT1,MT2> {
   static void f(
         Eigen::MatrixXd& cs,
         const Eigen::MatrixBase<MT1>& x1,
         const Eigen::MatrixBase<MT2>& x2);
};
template <typename MT1, typename MT2>
struct CoplanarConstraintHelper<3,MT1,MT2> {
   static void f(
         Eigen::MatrixXd& cs,
         const Eigen::MatrixBase<MT1>& x1,
         const Eigen::MatrixBase<MT2>& x2);
};

inline FivePointStewenius::FivePointStewenius(): _numSolutions(0)
{
}

template <typename MT1, typename MT2>
inline FivePointStewenius::FivePointStewenius(
      const Eigen::MatrixBase<MT1>& x1, const Eigen::MatrixBase<MT2>& x2, int opts):
   _numSolutions(0)
{
   compute(x1, x2, opts);
}

template <typename MT1, typename MT2>
inline void FivePointStewenius::compute(
      const Eigen::MatrixBase<MT1>& x1, const Eigen::MatrixBase<MT2>& x2, int opts)
{
   assert(x1.rows() == x2.rows());
   assert(x1.cols() == x2.cols());
   assert(x1.rows() >= 2 && x1.rows() <= 3);
   assert(x1.cols() >= 5);

   const int N = x1.cols();
   _epipolarConstraintMatrix.resize(9, N);

   CoplanarConstraintHelper<MT1::RowsAtCompileTime,MT1,MT2>::f(_epipolarConstraintMatrix, x1, x2);

   computeInternal(opts);
}

template <int Dim, typename MT1, typename MT2>
inline void CoplanarConstraintHelper<Dim,MT1,MT2>::f(
      Eigen::MatrixXd& cs,
      const Eigen::MatrixBase<MT1>& x1,
      const Eigen::MatrixBase<MT2>& x2)
{
   if (x1.rows() == 2) {
      CoplanarConstraintHelper<2,MT1,MT2>::f(cs, x1, x2);
   } else if (x1.rows() == 3) {
      CoplanarConstraintHelper<3,MT1,MT2>::f(cs, x1, x2);
   }
}

template <typename MT1, typename MT2>
inline void CoplanarConstraintHelper<2,MT1,MT2>::f(
      Eigen::MatrixXd& cs,
      const Eigen::MatrixBase<MT1>& x1,
      const Eigen::MatrixBase<MT2>& x2)
{
   const int N = x1.cols();
   for (int j = 0; j < N; ++j) {
      const Eigen::Vector2d a = x1.col(j);
      const Eigen::Vector2d b = x2.col(j);
      cs.col(j)
         << a(0)*b(0), a(0)*b(1), a(0),
            a(1)*b(0), a(1)*b(1), a(1),
              b(0)   ,   b(1)   , 1.0 ;
   }
}

template <typename MT1, typename MT2>
inline void CoplanarConstraintHelper<3,MT1,MT2>::f(
      Eigen::MatrixXd& cs,
      const Eigen::MatrixBase<MT1>& x1,
      const Eigen::MatrixBase<MT2>& x2)
{
   const int N = x1.cols();
   for (int j = 0; j < N; ++j) {
      const Eigen::Vector3d a = x1.col(j);
      const Eigen::Vector3d b = x2.col(j);
      cs.col(j)
         << a(0)*b(0), a(0)*b(1), a(0)*b(2),
            a(1)*b(0), a(1)*b(1), a(1)*b(2),
            a(2)*b(0), a(2)*b(1), a(2)*b(2);
   }
}

inline int FivePointStewenius::numSolutions() const
{
   return _numSolutions;
}

inline const Eigen::Matrix3d& FivePointStewenius::solution(int i) const
{
   assert(i >= 0 && i < _numSolutions);
   return _solutions[i];
}

} // namespace viewgeometry
} // namespace btl

#endif // BTL_VIEWGEOMETRY_FIVEPOINTPOSE
